#include "debug_print.h"
#include "stdarg.h"
#include "uart.h"
#include "uart1.h"

static DEBUG_TARGET_T s_debug_target = DEBUG_TO_UART_0;

DEBUG_TARGET_T debug_select_uart(DEBUG_TARGET_T uart)
{
  DEBUG_TARGET_T old = s_debug_target;
  s_debug_target = uart;
  return old;
}

void debug_puts(const uint8* str)
{
  if (str == 0)
    str = (const uint8*)"[null]";
  for (;;)
  {
    char c = *str;
    if (c == 0)
      break;
    debug_putc(c);
    str++;
  }
}

void debug_vprintf(const uint8* fmt, va_list vl)
{
  bool in_escape = false;

  for (;; fmt++)
  {
    char c = *fmt;
    if (c == 0)
      break;
    if (in_escape)
    {
      if (c == 'd')
        debug_print_int(va_arg(vl, uint16));
      else if (c == 'l')
        debug_print_uint32(va_arg(vl, uint32));
      else if (c == 'x')
        debug_print_hex(va_arg(vl, uint16));
      else if (c == 'c')
        debug_putc(va_arg(vl, uint8));
      else if (c == 's')
        debug_puts(va_arg(vl, uint8*));
      else if (c == 'f')
        debug_print_float(va_arg(vl, float));
      else if (c == 't')
        debug_print_time(va_arg(vl, DATE_TIME_T*));
      else
        debug_putc(c);
      in_escape = false;
      continue;
    }
    if (c == '%')
    {
      in_escape = true;
      continue;
    }
    debug_putc(c);
  }
  kick_watchdog();
}

void debug_printf(const uint8* fmt, ...)
{
  va_list vl;
  va_start(vl, fmt);

  debug_vprintf(fmt, vl);

  va_end(vl);
}

void debug_print_int(uint16 val)
{
  // max value of 16 bit int is 65535, 5 digits is enough
  uint8 buf[5];
  uint8 i = 0;

  do
  {
    uint8 v = val % 10 + '0';
    val /= 10;
    buf[i++] = v;
  } while (val > 0);
  while (i > 0)
    debug_putc(buf[--i]);
}

void debug_print_uint32(uint32 val)
{
  uint8 buf[10];
  uint8 i = 0;

  do
  {
    uint8 v = val % 10 + '0';
    val /= 10;
    buf[i++] = v;
  } while (val > 0);
  while (i > 0)
    debug_putc(buf[--i]);
}

void debug_print_float(float val)
{
  // this is NOT efficient, but works
  if (val < 0)
  {
    debug_putc('-');
    val = -val;
  }
  uint32 part_one = (uint32)val;
  uint32 part_two = (uint16)((val - (float)part_one) * 1000);
  debug_print_uint32(part_one);
  debug_putc('.');
  uint32 highest = 100;
  uint8 i;
  for (i = 0; i < 3; i++)
  {
    char c = (part_two / highest) + '0';
    debug_putc(c);
    part_two = part_two % highest;
    highest /= 10;
  }
}

void debug_print_hex(uint16 val)
{
  uint8 i;
  for (i = 12;; i -= 4)
  {
    uint8 v = (val >> i) & 0x0F;
    debug_putc(v < 10? (v + '0'): (v - 10 + 'A'));
    if (i == 0)
      break;
  }
}

void debug_print_time(DATE_TIME_T* time)
{
  debug_printf((const uint8*)"%d-%d-%d %d:%d:%d",
               time->year, time->month, time->date,
               time->hour, time->minute, time->second);
}

void debug_putc(uint8 c)
{
  void (*func)(uint8);
  func = (s_debug_target == DEBUG_TO_UART_0)? uart_putc: uart1_putc;

  if (c == '\n')
    func('\r');
  func(c);
}

void debug_echo(uint8 c)
{
  if (c == '\b')  // backspace
  {
    debug_puts((const uint8*)"\b \b");
  }
  else if (c == '\r')
  {
    debug_puts((const uint8*)"\r\n");
  }
  else
    debug_putc(c);
}

void mem_dump(uint8* ptr, uint16 size)
{
  uint16 i;
  uint8 v;
  debug_printf((const uint8*)"Dumping Memory: addr=0x%x, size=%d\n", ptr, size);
  for (i = 0; i < size; i++)
  {
    if ((i & 7) == 0)
      debug_printf((const uint8*)"\n0x%x: ", i);
    v = ptr[i];
    debug_print_hex(v);
    debug_putc(' ');
  }
  debug_putc('\n');
}
